/*
 * Motor_COntrol_Init.c
 *
 *  Created on: Mar 25, 2024
 *      Author: Yiwen Zhu
 */
#include "Motor_Control_Init.h"



// #############################################################
// #############################################################

CLARKE clarke1 = CLARKE_DEFAULTS;
IPARK ipark1 = IPARK_DEFAULTS;
SVGEN svm1 = SVGEN_DEFAULTS;
PIDREG3 pi_spd1 = PIDREG3_DEFAULTS;
PIDREG3 pi_id1 = PIDREG3_DEFAULTS;
PIDREG3 pi_iq1 = PIDREG3_DEFAULTS;
PARK	park1 = PARK_DEFAULTS;
SIMP_SVM simp_svm1 = SIMP_SVM_DEFAULTS;
MPCC	mpcc1 = MPCC_DEFAULTS;
MPDTC	mpdtc1 = MPDTC_DEFAULTS;
HFI	hfi1 = HFI_DEFAULTS;

// Global variables
int16_t AD_BUF[4] = {0};
volatile float G_Theta = 0;
volatile float G_We = 0;
volatile uint16_t G_Theta_Mech = 0;
char DEF_CTL_STATE = IDLE;	// Must initiation as IDLE



void STATE_ctl_commond(u8 state){
	if(state==START||state==RUN||state==STOP||state==IDLE)
		DEF_CTL_STATE = state;
	else
		DEF_CTL_STATE = STOP;
}

// Change PWM output state
void STATE_output_state(u8 state){
	if(state==STOP){
		PWM_EN = 0;
		// Enable and disable must be done separate for each channel
		__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_1, (0));    //修改比较值，修改占空比
		__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_2, (0));    //修改比较值，修改占空比
		__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_3, (0));    //修改比较值，修改占空比
	}
	else if(state==START){
		PWM_EN = 1;
	}
	else{
		return;
	}
}


void STATE_ctl_struc_init(void){

	pi_spd1.Ref = _IQ(0.3f);
	pi_spd1.Kp = SPD_KP;
	pi_spd1.Ki = SPD_KI;
	pi_spd1.Kd = _IQ(0);
	pi_spd1.Kc = _IQ(0.5);
	pi_spd1.OutMax = SPD_PI_MAX;
	pi_spd1.OutMin = SPD_PI_MIN;

	pi_id1.Ref = _IQ(0.0f);
	pi_id1.Kp = _IQ(ID_KP);
	pi_id1.Ki = _IQ(ID_KI);
	pi_id1.Kd = _IQ(0.0f);
	pi_id1.Kc = _IQ(0.6f);
	pi_id1.OutMax = (ID_PI_MAX);
	pi_id1.OutMin = (ID_PI_MIN);

	pi_iq1.Ref = _IQ(0.15f);
	pi_iq1.Kp = _IQ(IQ_KP);
	pi_iq1.Ki = _IQ(IQ_KI);
	pi_iq1.Kd = _IQ(0.0f);
	pi_iq1.Kc = _IQ(0.6f);
	pi_iq1.OutMax = (IQ_PI_MAX);
	pi_iq1.OutMin = (IQ_PI_MIN);

	simp_svm1.Udc = DRV_UDC;
	simp_svm1.KUdc = 0.66667f/simp_svm1.Udc;

	mpcc1.Ld = M_Ld;
	mpcc1.Lq = M_Lq;
	mpcc1.Rs = M_R;
	mpcc1.Psif = M_PSIr;
	mpcc1.Ts = (1.0f/(float)CONTROL_FRQ);
	mpcc1.g_opt = 99999999;
	mpcc_init();

	mpdtc1.Ld = M_Ld;
	mpdtc1.Lq = M_Lq;
	mpdtc1.Rs = M_R;
	mpdtc1.Psif = M_PSIr;
	mpdtc1.Np = M_NP;
	mpdtc1.Ts = (1.0f/(float)CONTROL_FRQ);
	mpdtc1.g_opt = 99999999;
	mpdtc1.Weight_Flux = WEIGHT_FACTOR_TRQUE;
	mpdtc_init();

	hfi1.Uinj = 1.5f;
	hfi1.LPF_alpha = 0.99f;
	hfi1.Kp = HFI_PLL_KP;	// 10
	hfi1.Ki = HFI_PLL_KI;	// 0.008

	six_puls1.Init_Flag = 0;
}


void STATE_clear_ctl_vrbs(void){

	pi_id1.Ui	= 0.0f;
	pi_iq1.Ui	= 0.0f;
	pi_spd1.Ui	 = 0.0f;
	hfi1.Ui = 0.0f;
//	pi_ang1.Ui	 = 0;
//	pi_ang1.Up1  = 0;

	six_puls1.Init_Flag = 0;

	mpcc1.Vd_Last = 0;
	mpcc1.Vq_Last = 0;
}

// #################  State-machine ##################
void STATE_motor_control_state_machine(u8 state){

	// Judge the protection event
	//overcur_protection(AD_BUF);
	HAL_DAC_SetValue(&hdac1, DAC1_CHANNEL_1, DAC_ALIGN_12B_R, (int)DEF_DA_DATA);
	HAL_DAC_SetValue(&hdac1, DAC1_CHANNEL_2, DAC_ALIGN_12B_R, (int)DEF_DA_DATA2);

	// Run state tasks
	switch(state){
	case STOP: // 暂态
		STATE_output_state(STOP); // Disable PWM output
		STATE_clear_ctl_vrbs();
		// Switch to the next state
		STATE_ctl_commond(IDLE);
		break;

	case IDLE:
		// Keep wait and clear the variables
		STATE_clear_ctl_vrbs();
		break;

	case START:// 暂态
		STATE_clear_ctl_vrbs();
		mpcc_init();
		mpdtc_init();
		STATE_output_state(START);// Enable PWM output
		// Switch to the next state
//		if(init_angle_meas()) STATE_ctl_commond(RUN);	// Six-Pulse
		if(init_angle_pull()) STATE_ctl_commond(RUN);	//
		break;

	case RUN:
		// Run main control task, defined in "Motor Control Init.h"
		DEF_MAIN_CONTROL_FUNCTION;
		break;

	}
}

